import smbus
import time
import serial
import logging


class PCA9685:
    # Registers/etc.
    __SUBADR1 = 0x02
    __SUBADR2 = 0x03
    __SUBADR3 = 0x04
    __MODE1 = 0x00
    __PRESCALE = 0xFE
    __LED0_ON_L = 0x06
    __LED0_ON_H = 0x07
    __LED0_OFF_L = 0x08
    __LED0_OFF_H = 0x09
    __ALLLED_ON_L = 0xFA  # 添加缺失的寄存器定义
    __ALLLED_ON_H = 0xFB  # 添加缺失的寄存器定义
    __ALLLED_OFF_L = 0xFC  # 添加缺失的寄存器定义
    __ALLLED_OFF_H = 0xFD  # 添加缺失的寄存器定义

    def __init__(self, address, debug=False):
        self.bus = smbus.SMBus(1)
        self.address = address
        self.debug = debug
        self.logger = logging.getLogger('PCA9685')
        self.last_angles = {}  # 存储各通道最后设置的角度

        if debug:
            self.logger.setLevel(logging.DEBUG)
        else:
            self.logger.setLevel(logging.INFO)

        formatter = logging.Formatter('%(asctime)s - %(name)s - %(levelname)s - %(message)s')
        ch = logging.StreamHandler()
        ch.setFormatter(formatter)
        self.logger.addHandler(ch)

        self.reset()  # 初始化时重置PCA9685
        self.set_pwm_freq(50)  # 设置PWM频率为50Hz，适用于舵机

    def reset(self):
        """重置PCA9685到初始状态"""
        self.write(self.__MODE1, 0x00)
        self.logger.info("PCA9685 reset")

    def write(self, reg, value):
        self.bus.write_byte_data(self.address, reg, value)
        if self.debug:
            self.logger.debug(f"I2C: Write 0x{value:02X} to register 0x{reg:02X}")

    def read(self, reg):
        result = self.bus.read_byte_data(self.address, reg)
        if self.debug:
            self.logger.debug(f"I2C: Device 0x{self.address:02X} returned 0x{result:02X} from reg 0x{reg:02X}")
        return result

    def set_pwm_freq(self, freq):
        """设置PWM频率"""
        prescaleval = 25000000.0 / 4096.0 / float(freq) - 1.0
        prescale = int(prescaleval + 0.5)

        oldmode = self.read(self.__MODE1)
        self.write(self.__MODE1, (oldmode & 0x7F) | 0x10)  # 进入睡眠模式
        self.write(self.__PRESCALE, prescale)  # 设置预分频值
        self.write(self.__MODE1, oldmode)  # 恢复之前的模式
        time.sleep(0.005)  # 等待振荡器稳定
        self.write(self.__MODE1, oldmode | 0x80)  # 启用自动重装载

        self.logger.info(f"Set PWM frequency to {freq} Hz")

    def set_pwm(self, channel, on, off):
        """设置指定通道的PWM值"""
        offset = channel * 4
        self.write(self.__LED0_ON_L + offset, on & 0xFF)
        self.write(self.__LED0_ON_H + offset, on >> 8)
        self.write(self.__LED0_OFF_L + offset, off & 0xFF)
        self.write(self.__LED0_OFF_H + offset, off >> 8)
        if self.debug:
            self.logger.debug(f"Set PWM for channel {channel}: ON={on}, OFF={off}")

    def set_servo_angle(self, channel, angle):
        """将舵机设置到指定角度"""
        # 确保角度在有效范围内
        angle = max(0, min(180, angle))

        # 使用与原始代码一致的角度-脉冲转换公式
        # 舵机角度范围 0-180 度，对应占空比 2.5-12.5%
        # PCA9685 的占空比范围是 0-4095
        pulse = int((angle / 18 + 2.5) / 100 * 4095)

        # 仅当角度变化超过1度时才更新PWM，减少不必要的操作
        if channel in self.last_angles and abs(angle - self.last_angles[channel]) < 1.0:
            return

        self.set_pwm(channel, 0, pulse)
        self.last_angles[channel] = angle  # 记录当前角度
        time.sleep(0.01)  # 减小延时，提高响应速度
        self.logger.info(f"Set servo angle for channel {channel}: {angle} degrees")


class PanTiltController:
    MODE_FOLLOW = 0  # 跟随模式（自动跟踪目标）
    MODE_MANUAL = 1  # 手动模式（直接设置角度）

    def __init__(self, horizontal_min, horizontal_max, vertical_min, vertical_max, center_threshold,
                 kp_pan, ki_pan, kp_tilt, ki_tilt, alpha, camera_flipped=False, debug=False):
        self.pca9685 = PCA9685(0x40, debug)
        self.horizontal_min = horizontal_min
        self.horizontal_max = horizontal_max
        self.vertical_min = vertical_min
        self.vertical_max = vertical_max
        self.center_threshold = center_threshold
        self.kp_pan = kp_pan
        self.ki_pan = ki_pan
        self.kp_tilt = kp_tilt
        self.ki_tilt = ki_tilt
        self.alpha = alpha
        self.integral_x = 0
        self.integral_y = 0
        self.current_pan = 90  # 水平初始位置
        self.current_tilt = 90  # 垂直初始位置
        self.camera_flipped = camera_flipped
        self.last_update_time = time.time()
        self.idle_timeout = 2.0  # 无操作超时时间（秒）
        self.current_mode = self.MODE_FOLLOW  # 默认使用跟随模式

        self.logger = logging.getLogger('PanTiltController')
        if debug:
            self.logger.setLevel(logging.DEBUG)
        else:
            self.logger.setLevel(logging.INFO)

        formatter = logging.Formatter('%(asctime)s - %(name)s - %(levelname)s - %(message)s')
        ch = logging.StreamHandler()
        ch.setFormatter(formatter)
        self.logger.addHandler(ch)

        # 摄像头图像尺寸(假设为QVGA 320x240)
        self.image_width = 320
        self.image_height = 240

        # 初始化舵机位置
        self.pca9685.set_servo_angle(9, self.current_pan)  # 水平舵机
        self.pca9685.set_servo_angle(10, self.current_tilt)  # 垂直舵机
        self.logger.info(f"Initialized pan to {self.current_pan} degrees, tilt to {self.current_tilt} degrees")

    def flip_coordinates(self, x, y):
        """翻转坐标以适应倒置的摄像头"""
        if not self.camera_flipped:
            return x, y

        # 水平翻转
        flipped_x = self.image_width - x
        # 垂直翻转
        flipped_y = self.image_height - y
        # 原点移至中心
        center_x = self.image_width // 2
        center_y = self.image_height // 2

        return flipped_x - center_x, flipped_y - center_y

    def parse_data(self, raw_data):
        """解析从串口接收的数据，支持多种命令格式"""
        try:
            data = raw_data.decode('utf-8', errors='ignore').strip()
            self.logger.debug(f"Received data: {data}")

            # 解析命令格式：$CMD,arg1,arg2#
            if data.startswith('$') and data.endswith('#'):
                parts = data[1:-1].split(',')
                cmd = parts[0].upper()
                args = parts[1:]

                if cmd == 'POS':  # 位置命令：$POS,pan_angle,tilt_angle#
                    if len(args) == 2:
                        try:
                            pan_angle = float(args[0])
                            tilt_angle = float(args[1])
                            return {
                                'command': 'set_position',
                                'pan': pan_angle,
                                'tilt': tilt_angle
                            }
                        except ValueError as ve:
                            self.logger.error(f"Failed to convert position values: {ve}, Data: {args}")

                elif cmd == 'MOVE':  # 相对移动命令：$MOVE,pan_delta,tilt_delta#
                    if len(args) == 2:
                        try:
                            pan_delta = float(args[0])
                            tilt_delta = float(args[1])
                            return {
                                'command': 'move',
                                'pan': pan_delta,
                                'tilt': tilt_delta
                            }
                        except ValueError as ve:
                            self.logger.error(f"Failed to convert move values: {ve}, Data: {args}")

                elif cmd == 'MODE':  # 模式切换命令：$MODE,mode_id#
                    if len(args) == 1:
                        try:
                            mode_id = int(args[0])
                            if mode_id in [self.MODE_FOLLOW, self.MODE_MANUAL]:
                                return {
                                    'command': 'set_mode',
                                    'mode': mode_id
                                }
                            else:
                                self.logger.error(f"Invalid mode ID: {mode_id}")
                        except ValueError as ve:
                            self.logger.error(f"Failed to convert mode ID: {ve}, Data: {args}")

                elif cmd == 'COORD':  # 坐标命令（兼容旧格式）：$COORD,x,y#
                    if len(args) == 2:
                        try:
                            x = int(args[0])
                            y = int(args[1])

                            # 将坐标转换为相对于图像中心的值
                            center_x = self.image_width // 2
                            center_y = self.image_height // 2
                            x_relative = x - center_x
                            y_relative = center_y - y  # y轴方向调整

                            # 应用坐标翻转
                            x_flipped, y_flipped = self.flip_coordinates(x_relative, y_relative)

                            return {
                                'command': 'coordinate',
                                'x': x_flipped,
                                'y': y_flipped
                            }
                        except ValueError as ve:
                            self.logger.error(f"Failed to convert coordinates: {ve}, Data: {args}")

                else:
                    self.logger.error(f"Unknown command: {cmd}")

            else:
                self.logger.error("Incorrect data format, missing $ at the beginning or # at the end")

        except Exception as e:
            self.logger.error(f"Data parsing failed: {e}, Raw data: {raw_data}")

        return None

    def process_command(self, command):
        """处理解析后的命令"""
        if command is None:
            return False

        cmd_type = command.get('command')

        if cmd_type == 'set_mode':
            self.current_mode = command['mode']
            mode_name = "FOLLOW" if self.current_mode == self.MODE_FOLLOW else "MANUAL"
            self.logger.info(f"Switched to {mode_name} mode")
            return True

        elif cmd_type == 'set_position':
            # 手动设置绝对位置
            pan_angle = max(self.horizontal_min, min(self.horizontal_max, command['pan']))
            tilt_angle = max(self.vertical_min, min(self.vertical_max, command['tilt']))

            if abs(pan_angle - self.current_pan) >= 1.0:
                self.current_pan = pan_angle
                self.pca9685.set_servo_angle(9, self.current_pan)
                self.logger.info(f"Manual pan set to: {self.current_pan:.1f}")

            if abs(tilt_angle - self.current_tilt) >= 1.0:
                self.current_tilt = tilt_angle
                self.pca9685.set_servo_angle(10, self.current_tilt)
                self.logger.info(f"Manual tilt set to: {self.current_tilt:.1f}")

            self.last_update_time = time.time()
            return True

        elif cmd_type == 'move':
            # 相对移动
            new_pan = self.current_pan + command['pan']
            new_tilt = self.current_tilt + command['tilt']

            new_pan = max(self.horizontal_min, min(self.horizontal_max, new_pan))
            new_tilt = max(self.vertical_min, min(self.vertical_max, new_tilt))

            if abs(new_pan - self.current_pan) >= 1.0:
                self.current_pan = new_pan
                self.pca9685.set_servo_angle(9, self.current_pan)
                self.logger.info(f"Manual pan moved to: {self.current_pan:.1f}")

            if abs(new_tilt - self.current_tilt) >= 1.0:
                self.current_tilt = new_tilt
                self.pca9685.set_servo_angle(10, self.current_tilt)
                self.logger.info(f"Manual tilt moved to: {self.current_tilt:.1f}")

            self.last_update_time = time.time()
            return True

        elif cmd_type == 'coordinate' and self.current_mode == self.MODE_FOLLOW:
            # 处理坐标数据（跟随模式）
            x = command['x']
            y = command['y']

            # 应用低通滤波
            x_filtered = self.alpha * x + (1 - self.alpha) * 0
            y_filtered = self.alpha * y + (1 - self.alpha) * 0

            # 水平控制（带死区检测）
            if abs(x_filtered) > self.center_threshold:
                self.integral_x += x_filtered
                self.integral_x = max(min(self.integral_x, 500), -500)
                pan_adjust = x_filtered * self.kp_pan + self.integral_x * self.ki_pan
                new_pan = self.current_pan - pan_adjust  # 注意符号：x增大时云台应向左转（减小角度）
                new_pan = max(min(new_pan, self.horizontal_max), self.horizontal_min)
                if abs(new_pan - self.current_pan) >= 1.0:  # 变化超过1度才更新
                    self.current_pan = new_pan
                    self.pca9685.set_servo_angle(9, self.current_pan)  # 水平舵机通道9
                    self.logger.info(f"Pan adjusted to: {self.current_pan:.1f}")

            # 垂直控制（带死区检测）
            if abs(y_filtered) > self.center_threshold:
                self.integral_y += y_filtered
                self.integral_y = max(min(self.integral_y, 500), -500)
                tilt_adjust = y_filtered * self.kp_tilt + self.integral_y * self.ki_tilt
                new_tilt = self.current_tilt + tilt_adjust  # 注意符号：y增大时云台上仰（增大角度）
                new_tilt = max(min(new_tilt, self.vertical_max), self.vertical_min)
                if abs(new_tilt - self.current_tilt) >= 1.0:  # 变化超过1度才更新
                    self.current_tilt = new_tilt
                    self.pca9685.set_servo_angle(10, self.current_tilt)  # 垂直舵机通道10
                    self.logger.info(f"Tilt adjusted to: {self.current_tilt:.1f}")

            self.last_update_time = time.time()
            return True

        return False

    def control(self):
        """控制舵机的主循环"""
        ser = serial.Serial(
            port='/dev/ttyS0',
            baudrate=9600,
            timeout=1,
            parity=serial.PARITY_NONE,
            stopbits=serial.STOPBITS_ONE,
            bytesize=serial.EIGHTBITS
        )

        self.logger.info("Starting pan-tilt control loop")

        try:
            while True:
                current_time = time.time()
                has_data = False

                if ser.in_waiting > 0:
                    buffer = ser.read(ser.in_waiting)
                    if b'#' in buffer:
                        parts = buffer.split(b'#', 1)
                        raw_data = parts[0] + b'#'

                        # 解析命令
                        command = self.parse_data(raw_data)

                        # 处理命令
                        has_data = self.process_command(command)

                # 无数据时的归中逻辑（带超时检测）
                if not has_data and (current_time - self.last_update_time) > self.idle_timeout:
                    if abs(self.current_pan - 90) > 1.0 or abs(self.current_tilt - 90) > 1.0:
                        delta_pan = (90 - self.current_pan) * 0.2  # 降低归中速度
                        delta_tilt = (90 - self.current_tilt) * 0.2
                        self.current_pan += delta_pan
                        self.current_tilt += delta_tilt
                        self.pca9685.set_servo_angle(9, int(self.current_pan))  # 水平舵机归中
                        self.pca9685.set_servo_angle(10, int(self.current_tilt))  # 垂直舵机归中
                        self.logger.debug(f"Homing: Pan={int(self.current_pan)}, Tilt={int(self.current_tilt)}")

                time.sleep(0.01)  # 减小主循环延时，提高响应速度

        except KeyboardInterrupt:
            self.logger.info("Program terminated by user")
        finally:
            # 程序结束时重置舵机到初始位置
            self.logger.info("Resetting servos to home position")
            self.pca9685.set_servo_angle(9, 90)
            self.pca9685.set_servo_angle(10, 90)
            time.sleep(0.5)  # 等待舵机移动到位
            ser.close()  # 关闭串口连接


if __name__ == "__main__":
    # 配置参数
    horizontal_min = 20  # 水平最小角度
    horizontal_max = 160  # 水平最大角度
    vertical_min = 30  # 垂直最小角度
    vertical_max = 150  # 垂直最大角度
    center_threshold = 15  # 中心死区阈值，小于此值不调整
    kp_pan = 1.0  # 水平方向比例增益
    ki_pan = 0.01  # 水平方向积分增益
    kp_tilt = 1.0  # 垂直方向比例增益
    ki_tilt = 0.01  # 垂直方向积分增益
    alpha = 0.3  # 低通滤波系数
    camera_flipped = True  # 设置为True表示摄像头倒置

    # 创建并启动控制器
    controller = PanTiltController(
        horizontal_min, horizontal_max, vertical_min, vertical_max,
        center_threshold, kp_pan, ki_pan, kp_tilt, ki_tilt,
        alpha, camera_flipped, debug=True
    )

    controller.control()